namespace CANNON
{
	export class GridBroadphase extends Broadphase
	{
		nx: number;
		ny: number;
		nz: number;
		aabbMin: Vec3;
		aabbMax: Vec3;
		bins: any[];
		binLengths: any[]; //Rather than continually resizing arrays (thrashing the memory), just record length and allow them to grow

		/**
		 * Axis aligned uniform grid broadphase.
		 * 
		 * @param aabbMin
		 * @param aabbMax
		 * @param nx Number of boxes along x
		 * @param ny Number of boxes along y
		 * @param nz Number of boxes along z
		 * 
		 * @todo Needs support for more than just planes and spheres.
		 */
		constructor(aabbMin: Vec3, aabbMax: Vec3, nx: number, ny: number, nz: number)
		{
			super();
			this.nx = nx || 10;
			this.ny = ny || 10;
			this.nz = nz || 10;
			this.aabbMin = aabbMin || new Vec3(100, 100, 100);
			this.aabbMax = aabbMax || new Vec3(-100, -100, -100);
			var nbins = this.nx * this.ny * this.nz;
			if (nbins <= 0)
			{
				throw "GridBroadphase: Each dimension's n must be >0";
			}
			this.bins = [];
			this.binLengths = []; //Rather than continually resizing arrays (thrashing the memory), just record length and allow them to grow
			this.bins.length = nbins;
			this.binLengths.length = nbins;
			for (var i = 0; i < nbins; i++)
			{
				this.bins[i] = [];
				this.binLengths[i] = 0;
			}
		}

		/**
		 * Get all the collision pairs in the physics world
		 * 
		 * @param world
		 * @param pairs1
		 * @param pairs2
		 */
		collisionPairs(world: World, pairs1: any[], pairs2: any[])
		{
			var N = world.numObjects(),
				bodies = world.bodies;

			var max = this.aabbMax,
				min = this.aabbMin,
				nx = this.nx,
				ny = this.ny,
				nz = this.nz;

			var xstep = ny * nz;
			var ystep = nz;
			var zstep = 1;

			var xmax = max.x,
				ymax = max.y,
				zmax = max.z,
				xmin = min.x,
				ymin = min.y,
				zmin = min.z;

			var xmult = nx / (xmax - xmin),
				ymult = ny / (ymax - ymin),
				zmult = nz / (zmax - zmin);

			var binsizeX = (xmax - xmin) / nx,
				binsizeY = (ymax - ymin) / ny,
				binsizeZ = (zmax - zmin) / nz;

			var binRadius = Math.sqrt(binsizeX * binsizeX + binsizeY * binsizeY + binsizeZ * binsizeZ) * 0.5;

			var types = Shape.types;
			var SPHERE = types.SPHERE,
				PLANE = types.PLANE,
				BOX = types.BOX,
				COMPOUND = types.COMPOUND,
				CONVEXPOLYHEDRON = types.CONVEXPOLYHEDRON;

			var bins = this.bins,
				binLengths = this.binLengths,
				Nbins = this.bins.length;

			// Reset bins
			for (var i = 0; i !== Nbins; i++)
			{
				binLengths[i] = 0;
			}

			var ceil = Math.ceil;
			// var min = Math.min;
			// var max = Math.max;

			function addBoxToBins(x0, y0, z0, x1, y1, z1, bi)
			{
				var xoff0 = ((x0 - xmin) * xmult) | 0,
					yoff0 = ((y0 - ymin) * ymult) | 0,
					zoff0 = ((z0 - zmin) * zmult) | 0,
					xoff1 = ceil((x1 - xmin) * xmult),
					yoff1 = ceil((y1 - ymin) * ymult),
					zoff1 = ceil((z1 - zmin) * zmult);

				if (xoff0 < 0) { xoff0 = 0; } else if (xoff0 >= nx) { xoff0 = nx - 1; }
				if (yoff0 < 0) { yoff0 = 0; } else if (yoff0 >= ny) { yoff0 = ny - 1; }
				if (zoff0 < 0) { zoff0 = 0; } else if (zoff0 >= nz) { zoff0 = nz - 1; }
				if (xoff1 < 0) { xoff1 = 0; } else if (xoff1 >= nx) { xoff1 = nx - 1; }
				if (yoff1 < 0) { yoff1 = 0; } else if (yoff1 >= ny) { yoff1 = ny - 1; }
				if (zoff1 < 0) { zoff1 = 0; } else if (zoff1 >= nz) { zoff1 = nz - 1; }

				xoff0 *= xstep;
				yoff0 *= ystep;
				zoff0 *= zstep;
				xoff1 *= xstep;
				yoff1 *= ystep;
				zoff1 *= zstep;

				for (var xoff = xoff0; xoff <= xoff1; xoff += xstep)
				{
					for (var yoff = yoff0; yoff <= yoff1; yoff += ystep)
					{
						for (var zoff = zoff0; zoff <= zoff1; zoff += zstep)
						{
							var idx = xoff + yoff + zoff;
							bins[idx][binLengths[idx]++] = bi;
						}
					}
				}
			}

			// Put all bodies into the bins
			for (var i = 0; i !== N; i++)
			{
				var bi = bodies[i];
				var si = bi.shape;

				switch (si.type)
				{
					case SPHERE:
						// Put in bin
						// check if overlap with other bins
						var x = bi.position.x,
							y = bi.position.y,
							z = bi.position.z;
						var r = si.radius;

						addBoxToBins(x - r, y - r, z - r, x + r, y + r, z + r, bi);
						break;

					case PLANE:
						if (si.worldNormalNeedsUpdate)
						{
							si.computeWorldNormal(bi.quaternion);
						}
						var planeNormal = si.worldNormal;

						//Relative position from origin of plane object to the first bin
						//Incremented as we iterate through the bins
						var xreset = xmin + binsizeX * 0.5 - bi.position.x,
							yreset = ymin + binsizeY * 0.5 - bi.position.y,
							zreset = zmin + binsizeZ * 0.5 - bi.position.z;

						var d = GridBroadphase_collisionPairs_d;
						d.set(xreset, yreset, zreset);

						for (var xi = 0, xoff = 0; xi !== nx; xi++ , xoff += xstep, d.y = yreset, d.x += binsizeX)
						{
							for (var yi = 0, yoff = 0; yi !== ny; yi++ , yoff += ystep, d.z = zreset, d.y += binsizeY)
							{
								for (var zi = 0, zoff = 0; zi !== nz; zi++ , zoff += zstep, d.z += binsizeZ)
								{
									if (d.dot(planeNormal) < binRadius)
									{
										var idx = xoff + yoff + zoff;
										bins[idx][binLengths[idx]++] = bi;
									}
								}
							}
						}
						break;

					default:
						if (bi.aabbNeedsUpdate)
						{
							bi.computeAABB();
						}

						addBoxToBins(
							bi.aabb.lowerBound.x,
							bi.aabb.lowerBound.y,
							bi.aabb.lowerBound.z,
							bi.aabb.upperBound.x,
							bi.aabb.upperBound.y,
							bi.aabb.upperBound.z,
							bi);
						break;
				}
			}

			// Check each bin
			for (var i = 0; i !== Nbins; i++)
			{
				var binLength = binLengths[i];
				//Skip bins with no potential collisions
				if (binLength > 1)
				{
					var bin = bins[i];

					// Do N^2 broadphase inside
					for (var xi = 0; xi !== binLength; xi++)
					{
						var bi = bin[xi];
						for (var yi = 0; yi !== xi; yi++)
						{
							var bj = bin[yi];
							if (this.needBroadphaseCollision(bi, bj))
							{
								this.intersectionTest(bi, bj, pairs1, pairs2);
							}
						}
					}
				}
			}

			//	for (var zi = 0, zoff=0; zi < nz; zi++, zoff+= zstep) {
			//		console.log("layer "+zi);
			//		for (var yi = 0, yoff=0; yi < ny; yi++, yoff += ystep) {
			//			var row = '';
			//			for (var xi = 0, xoff=0; xi < nx; xi++, xoff += xstep) {
			//				var idx = xoff + yoff + zoff;
			//				row += ' ' + binLengths[idx];
			//			}
			//			console.log(row);
			//		}
			//	}

			this.makePairsUnique(pairs1, pairs2);
		}
	}


	var GridBroadphase_collisionPairs_d = new Vec3();
	var GridBroadphase_collisionPairs_binPos = new Vec3();
}